#include <cv.h>
#include <highgui.h>
#include <stdio.h>
#include <string>
#include <iostream>

#include <fstream>

using namespace cv;
using namespace std;

double tailleUniteMire = 27;

int main(int argc, char *argv[] )
{
    cv::VideoCapture cap(std::string("mire1_4x3x27mm.avi"));
    if(!cap.isOpened())  // check if we succeeded
        return -1;

    int mirParX = atoi(argv[1]), mirParY = atoi(argv[2]);

    cv::namedWindow("Pattern",CV_WINDOW_AUTOSIZE);

    vector<Point3f> v3f;
    for(int i = 0; i < 3; i++)
        for(int j = 0; j < 4; j++)
            v3f.push_back(Point3f(tailleUniteMire*i,tailleUniteMire*j,0));

    vector<vector<Point2f> > imagePoints;
    vector<vector<Point3f> > objectPoints;  //positions des coins de la mire dans l'espace

    Mat cameraMatrix(3, 3, DataType<double>::type);
    Mat distCoeffs  (1, 4, DataType<double>::type);

    bool coordExtrinseques = false;
    cout << "Chargement des coordonnees intrinseques. " << endl;

    ifstream fCoordIntrin("intrinseques.txt");

    if(fCoordIntrin)
    {
        string mot;
        double val;

        fCoordIntrin >> mot;
        for(int i = 0; i < 4; i ++)
        {
            fCoordIntrin >> val;
            distCoeffs.at<double>(0,i) = val;
        }

        fCoordIntrin >> mot;
        for(int r = 0; r < cameraMatrix.rows ; r++)
        {
            for(int c = 0; c < cameraMatrix.cols ; c++)
               {
                 fCoordIntrin >> val;
                 cameraMatrix.at<double>(r,c) = val;

               }
        }
    }


    Mat frame,img;
    int imgX=0,imgY=0;

    Mat rvecs, tvecs, rvecsM;

    bool calculExtrinseque = false;

    int numI = 0;
    while(cap.grab())
    {
        cap >> frame; // get a new frame from camera
        cap >> img;


        char key = ' ';
        key =  cv::waitKey(50);

        if(imgX == 0) {imgX = frame.rows; imgY = frame.cols;}

        Size patternsize(mirParX,mirParY); //interior number of corners
        vector<Point2f> corners;           //this will be filled by the detected corners

        bool patternfound = findChessboardCorners(frame, patternsize, corners,
                CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE);

        objectPoints.push_back(v3f);
        imagePoints.push_back(corners);

        if(patternfound)
            drawChessboardCorners(img, patternsize, Mat(corners), patternfound);

        Mat imagePointsM(corners);
        Mat objectPointsM(v3f);


        numI++;
        if((patternfound || corners.size() >0) && calculExtrinseque == false) // verif
        {
            solvePnP(objectPointsM,imagePointsM,cameraMatrix,distCoeffs,rvecs,tvecs, false);
            calculExtrinseque = true;
            Rodrigues(rvecs, rvecsM);

            cout << "Calculs extrinseques effectués. " << endl;
        }


        cv::imshow( "Pattern",img);

        if(key == 'q')
              break;
    }


    while(true)
    {
        char key = ' ';
        key = cv::waitKey(200);

        if (key == 'a')
        {
            for(int i = 0; i < rvecsM.rows; i++)
            {
                for(int j = 0; j < rvecsM.cols; j++)
                     cout << rvecsM.at<double>(i,j)<< " ";


                cout << endl;
            }

            cout << "tvecs: " << endl;
            cout << tvecs.rows << " " << tvecs.cols << endl;
            for(int i = 0; i < tvecs.rows; i++)
            {
                for(int j = 0; j < tvecs.cols; j++)
                        cout << tvecs.at<double>(i,j)<< " ";

                cout << endl;
            }

        }
        else if (key == 'w' )
        {
            ofstream fCoordExtrin("extrinseques.txt");

            if(fCoordExtrin)
            {
                fCoordExtrin << "rotation:";

                for(int r = 0; r < rvecsM.rows ; r++)
                {
                    fCoordExtrin << endl;
                    for(int c = 0; c < rvecsM.cols ; c++)
                        fCoordExtrin <<   rvecsM.at<double>(r,c) << " ";
                }
                fCoordExtrin << endl << "translation:" << endl;

                for(int i = 0; i < 3; i ++)
                        fCoordExtrin <<  tvecs.at<double>(0,i) << " ";

                cout << "Coordonnees extrinseques sauvegardees dans extrinseques.txt." << endl;
            }
        }
//
//
//
        if(key == 'q')
              break;
    }
    cout << endl;
    return 0;
}

/// Acquisition temps-réel de mire
//int main(int argc, char *argv[] )
//{
//    cv::VideoCapture cap(std::string("mire2_4x3x27mm.avi"));
//    if(!cap.isOpened())  // check if we succeeded
//        return -1;
//
//    int mirParX = atoi(argv[1]), mirParY = atoi(argv[2]);
//
////    cv::namedWindow("Video");
//    cv::namedWindow("Pattern");
//
//    for(;;)
//    {
//        Mat frame,img;
//        cap >> frame; // get a new frame from camera
//        cap >> img;
//        char key = ' ';
//        key =  cv::waitKey(100);
////        cv::imshow( "Video",frame);
//
//        Size patternsize(mirParX,mirParY); //interior number of corners
//        vector<Point2f> corners; //this will be filled by the detected corners
//
//        bool patternfound = findChessboardCorners(frame, patternsize, corners,
//                CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE);
//
//        if(patternfound)
//            drawChessboardCorners(img, patternsize, Mat(corners), patternfound);
//
//        cv::imshow( "Pattern",img);
//
//        if(key == 'q')
//              break;
//    }
//
//    return 0;
//}
///Calibrage intrinsèque
//
//int main(int argc, char *argv[] )
//{
//    cv::VideoCapture cap(std::string("mire1_4x3x27mm.avi"));
//    if(!cap.isOpened())  // check if we succeeded
//        return -1;
//
//    int mirParX = atoi(argv[1]), mirParY = atoi(argv[2]);
//
////    cv::namedWindow("Video");
//    cv::namedWindow("Pattern",CV_WINDOW_AUTOSIZE);
//
//    vector<Point3f> v3f;
//    for(int i = 0; i < 3; i++)
//        for(int j = 0; j < 4; j++)
//            v3f.push_back(Point3f(i,j,0));
//
//    vector<vector<Point2f> > imagePoints;
//    vector<vector<Point3f> > objectPoints;  //positions des coins de la mire dans l'espace
//
//     Mat frame,img;
//     int imgX=0,imgY=0;
//
//
//
//    while(cap.grab() )
//    {
//        cap >> frame; // get a new frame from camera
//        cap >> img;
//
//        char key = ' ';
//        key =  cv::waitKey(200);
//
//        if(imgX == 0) {imgX = frame.rows; imgY = frame.cols;}
//
//        Size patternsize(mirParX,mirParY); //interior number of corners
//        vector<Point2f> corners;           //this will be filled by the detected corners
//
//        bool patternfound = findChessboardCorners(frame, patternsize, corners,
//                CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE);
//
//        objectPoints.push_back(v3f);
//        imagePoints.push_back(corners);
//
//        if(patternfound)
//            drawChessboardCorners(img, patternsize, Mat(corners), patternfound);
//
//        cv::imshow( "Pattern",img);
//
//        if(key == 'q')
//              break;
//    }
//
//    Mat cameraMatrix,distCoeffs;
//    vector<Mat> rvecs, tvecs;
//
//    bool coordIntrinsequesCalculees = false;
//
//    while(true)
//    {
//        char key = ' ';
//        key = cv::waitKey(200);
//
//        if(key == 'c')
//        {
//            calibrateCamera(objectPoints,imagePoints, Size(imgX,imgY), cameraMatrix,distCoeffs, rvecs, tvecs);
//            cout << "Coordonnées calculées. " << endl;
//            coordIntrinsequesCalculees = true;
//        }
//        else if (key == 'd' && coordIntrinsequesCalculees)
//        {
//            cout << "Camera matrix: " << endl;
//            for(int r = 0; r < cameraMatrix.rows ; r++)
//            {
//                cout << endl;
//                for(int c = 0; c < cameraMatrix.cols ; c++)
//                    cout <<   cameraMatrix.at<double>(r,c) << " ";
//            }
//
//            cout << "Valeurs distorsion: " << endl;
//            cout << "k1 " << distCoeffs.at<double>(0,0) << " k2 " << distCoeffs.at<double>(0,1)
//                << " p1 " << distCoeffs.at<double>(0,2) << " p2 " << distCoeffs.at<double>(0,3) << endl;
//        }
//        else if (key == 's' && coordIntrinsequesCalculees)
//        {
//            ofstream fCoordIntrin("intrinseques.txt");
//
//            if(fCoordIntrin)
//            {
//                fCoordIntrin << "distortion:" << endl;
//                for(int i = 0; i < 4; i ++)
//                        fCoordIntrin <<  distCoeffs.at<double>(0,i) << " ";
//
//                fCoordIntrin << endl << "projection:";
//
//                for(int r = 0; r < cameraMatrix.rows ; r++)
//                {
//                    fCoordIntrin << endl;
//                    for(int c = 0; c < cameraMatrix.cols ; c++)
//                        fCoordIntrin <<   cameraMatrix.at<double>(r,c) << " ";
//                }
//
//                cout << "Coordonnees intrinseques sauvegardees dans intrinseques.txt." << endl;
//            }
//        }
//
//
//
//        if(key == 'q')
//              break;
//    }
//    cout << endl;
//    return 0;
//}
